package pantomime.hardware;
import java.util.HashMap;
import java.util.Iterator;

/**
 * @author PPatino
 * Defines an object that will allow us to conveniently interact with our Inertial Measurement Unit (IMU).
 *
 */
public class IMU {

	private HashMap _IMUSensors;

	public enum Sensor{

		xAcc("xAcc"),
		yAcc("yAcc"),
		zAcc("zAcc"),
		xGyro("xGyro"),
		yGyro("yGyro"),
		zGyro("zGyro");
		
		private String sName;
		
		Sensor(String sName){
			this.sName = sName;
		}
		
		public static Sensor fromString(String sName){
			if(sName != null){
				for(Sensor sSensor : Sensor.values()){
					if(sName.equalsIgnoreCase(sSensor.sName)){
						return sSensor;
					}
				}
			}
			throw new IllegalArgumentException("Invalid sensor name passed into 'eSensor' constructor: sName=" + sName);
		}
	}
	
	public IMU(float xAcc, float yAcc ,float zAcc, float xGyro, float yGyro, float zGyro){
		_IMUSensors = new HashMap();
		setIMUSensorValues(xAcc, yAcc, zAcc, xGyro, yGyro, zGyro);		
	}
	
	public IMU(){
		_IMUSensors = new HashMap();
		//Initialize to an invalid value
		setIMUSensorValues(-1, -1, -1, -1, -1, -1);
	}

	public void setIMUSensorValues(float xAcc, float yAcc ,float zAcc, float xGyro, float yGyro, float zGyro){
		_IMUSensors.put(Sensor.xAcc.toString(), xAcc);
		_IMUSensors.put(Sensor.yAcc.toString(), yAcc);
		_IMUSensors.put(Sensor.zAcc.toString(), zAcc);
		_IMUSensors.put(Sensor.xGyro.toString(), xGyro);
		_IMUSensors.put(Sensor.yGyro.toString(), yGyro);
		_IMUSensors.put(Sensor.zGyro.toString(), zGyro);
	}
	
	public int getIMUValueByName(Sensor eSensor){
		return (int) Float.parseFloat(_IMUSensors.get(eSensor.name()).toString());
	}
	
	public void setIMUValueByName(Sensor eSensor, float value){
		_IMUSensors.put(eSensor.toString(), value);
	}
	
	//TODO:
	//For now, we set the Gyro values to 0 when 'normalizing' - we don't care about them for static gestures.
	public void normalizeIMU(){
		Iterator i = _IMUSensors.keySet().iterator();
		while(i.hasNext()){
			String key = (String) i.next();
			float fNormalizedValue = normalizeIMUValue(Float.parseFloat(_IMUSensors.get(key).toString()));
			_IMUSensors.put(key, fNormalizedValue);
		}
		
		_IMUSensors.put(Sensor.xGyro.toString(), 0.0);
		_IMUSensors.put(Sensor.yGyro.toString(), 0.0);
		_IMUSensors.put(Sensor.zGyro.toString(), 0.0);
	}
	
	private float normalizeIMUValue(float fOldValue){
		if(fOldValue < -0.5){
			return -1.0f;
		}
		else if(fOldValue >= -0.5 && fOldValue <= 0.5){
			return 0.0f;
		}
		else if(fOldValue > 0.5){
			return 1.0f;
		}
		
		//never executes.
		return 0;

	}
	
	public String toString(){
		String sToString = "IMU [";
		for(Sensor currentSensor : Sensor.values()){
			sToString += currentSensor.toString() + "=" + _IMUSensors.get(currentSensor.toString()) + ", ";
		}
		sToString += "]";
		
		return sToString;
	}
	
	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result
				+ ((_IMUSensors == null) ? 0 : _IMUSensors.hashCode());
		return result;
	}

	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		IMU other = (IMU) obj;
		if (_IMUSensors == null) {
			if (other._IMUSensors != null)
				return false;
		}
		
		/*else if (!_IMUSensors.equals(other._IMUSensors))
			return false;
		return true;*/
		//TODO: I switched the signs because they are reversed in XML vs recognition.
		else if(!( this.getIMUValueByName(Sensor.fromString("xAcc")) ==  - other.getIMUValueByName(Sensor.fromString("xAcc")) &&
				this.getIMUValueByName(Sensor.fromString("yAcc")) ==  other.getIMUValueByName(Sensor.fromString("yAcc")) &&
				this.getIMUValueByName(Sensor.fromString("zAcc")) ==  - other.getIMUValueByName(Sensor.fromString("zAcc")) &&
				this.getIMUValueByName(Sensor.fromString("xGyro")) ==  other.getIMUValueByName(Sensor.fromString("xGyro")) &&
				this.getIMUValueByName(Sensor.fromString("yGyro")) ==  other.getIMUValueByName(Sensor.fromString("yGyro")) &&
				this.getIMUValueByName(Sensor.fromString("zGyro")) ==  other.getIMUValueByName(Sensor.fromString("zGyro"))
				)) return false;
		
		return true;
	}
	
}
